import numpy as np
import time

# TODO:
'''
1.检测矩形四条边
2.计算矩形顶点
3.保存透视变化矩阵 
4.小球坐标

'''
min_length_of_line = 150  # a line too short will be flitted
min_width_of_ls_to_merge = 30  # when 2 lines where distance between them is lower than this
# , when paralleled, will be merged to one line
max_distance_of_ls_to_joint = 100  # to joint 2 paralleled lines
max_basis_in_same_line = 30  # paralleled lines seg, and in the same line
max_angle_for_paralleled = 10 * np.pi / 180  # 10 deg, when two line cross within this angle will be seemed paralleled
min_gap_for_doubling_lines = 100  # min width for two paralleled lines in the rectangle

max_angle_to_remove = 15 * np.pi / 180  # if a ls in angle above it will be removed


def kb(ls):
    '''
    y = kx + b
    :param ls:[xyxy]
    :return: [k,b] or [np.nan,0]
    '''
    [x1, y1, x2, y2] = ls
    k = 0
    c = 0
    if x2 != x1:
        k = (y2 - y1) / (x2 - x1)
        c = y2 - k * x2
    else:
        k = np.nan
    return [k, c]


def box_edge_margin(ls):
    pass


def distance_p_p(p1, p2):
    '''
    distance between two points
    :param p1: [x,y]
    :param p2: [x,y]
    :return:
    '''
    p1 = np.array(p1)
    p2 = np.array(p2)
    a = np.square(p1 - p2)
    return np.sqrt(np.sum(a))


def length_of_ls(line):
    '''
    a length of a line segment
    :param line: [x y x y]
    :return: sqrt dx^2 + dy^2
    '''
    if len(line) == 4:
        [a, b, c, d] = line
        return ((a - c) ** 2 + (b - d) ** 2) ** 0.5
    elif len(line[0]) == 1:
        [a, b, c, d] = line[0]
        return ((a - c) ** 2 + (b - d) ** 2) ** 0.5


def distance_p_l(p, l):
    '''
    distance between a line(not a line segment) and a point,
    :param p: [[x,y]],[x,y]
    :param l: [x1,y1,x2,y2],[[xxxx]]
    :return: abs distance
    '''
    if len(l) == 1:
        l = l[0]
    assert len(l) == 4
    if len(p) == 1:
        p = p[0]
    assert len(p) == 2
    [x1, y1, x2, y2] = l
    [x0, y0] = p
    if x1 == x2:
        return abs(x0 - x1)
    elif y1 == y2:
        return abs(y1 - y0)
    k = (y1 - y2) / (x1 - x2)
    b = y1 - k * x1
    d = k * x0 - y0 + b
    d = d / (1 + k ** 2) ** 0.5
    return d


def distance_p_ls(p, ls):
    '''
    distance between a line segment and a point
    :param p:[x0,y0],[[xy]]
    :param ls:[x1,y1,x2,y2],[[xyxy]]
    :return: min(d1,d2)
    '''
    # if len(ls) == 1:
    #     ls = ls[0]
    # assert len(ls) == 4
    d1 = distance_p_p(p, ls[[0, 1]])
    d2 = distance_p_p(p, ls[[2, 3]])
    return min(d1, d2)


def distance_ls_ls(l1, l2):
    '''
    distance between 2 line sgements
    :param l1: [x1,y1,x2,y2]
    :param l2:[x y x y]
    :return:min([d1,d2,d3,d4])
    '''
    # l1 = np.reshape(l1, 4)
    # l2 = np.reshape(l2, 4)
    d1 = distance_p_ls(l1[[0, 1]], l2)
    d2 = distance_p_ls(l1[[2, 3]], l2)
    # d3 = distance_p_l(l2[0, 1], l1)
    # d4 = distance_p_l(l2[2, 3], l1)
    return np.min([d1, d2])


def theta_ls(line):
    '''
    angle of a line segment
    :param line: [x y x y]
    :return: (-pi/2,pi/2]
    '''
    if len(line) == 1:
        line = line[0]
    assert len(line) == 4
    theta = 0
    [x1, y1, x2, y2] = line
    if x1 - x2 == 0:
        theta = np.pi / 2
    else:
        theta = np.arctan((y1 - y2) / (x1 - x2))
    return theta


def delta_theta_ls(l1, l2):
    a1 = theta_ls(l1)
    a2 = theta_ls(l2)
    return np.abs(a1 - a2)


def remove_short_ls(lines):
    """
    flitter for min_length_of_line
    :param lines: np.array nx4
    :return: remove count
    """
    c = 0
    for i in range(len(lines)):
        if length_of_ls(lines[i]) < min_length_of_line:
            lines[i] = 0
            c += 1
    return c


def basi_ls_in_same_line(l1, l2):
    '''
    get the basi for 2 ls in the same line, if they are paralleled
    :param l1:
    :param l2:
    :return: max(d1,d2)
    '''
    d1 = abs(distance_p_l(l1[[0, 1]], l2))
    d2 = abs(distance_p_l(l1[[2, 3]], l2))
    return max(d1, d2)


def joint_ls(l1, l2):
    '''
    joint 2 lines seg into one
    :param l1: [xyxy]
    :param l2: [xyxy]
    :return: [xyxy]
    '''
    l = np.array(l1)

    d1 = distance_p_ls(l1[[0, 1]], l2)
    d2 = distance_p_ls(l1[[2, 3]], l2)
    if d1 > d2:  # l1 [x0,y0] far away from l2
        l[[0, 1]] = l1[[0, 1]]
    else:  # l1 [x1,y1] far away from l2
        l[[0, 1]] = l1[[2, 3]]

    d1 = distance_p_ls(l2[[0, 1]], l1)
    d2 = distance_p_ls(l2[[2, 3]], l1)
    if d1 > d2:
        l[[2, 3]] = l2[[0, 1]]
    else:
        l[[2, 3]] = l2[[2, 3]]

    return l


def merge_ls(l1, l2):
    '''
    merge 2 ling seg, a longer one will displace to the center
    :param l1: [xyxy]
    :param l2: [xyxy]
    :return: [xyxy]
    '''
    length1 = length_of_ls(l1)
    length2 = length_of_ls(l2)
    if length1 < length2:
        t = l1
        l1 = l2
        l2 = t
        length1 = length2
    # now l1 > l2
    p1 = [l1[2] - l1[0], l1[3] - l1[1]]
    p2 = np.array([p1[1], -1 * p1[0]], 'float')
    d1 = distance_p_l(l2[[0, 1]], l1)
    d2 = distance_p_l(l2[[2, 3]], l1)
    d = (d1 + d2) / 4
    k = d / length1
    p2 *= k  # oneness
    l = np.array(l1, 'float')
    l[[0, 1]] += p2
    l[[2, 3]] += p2
    return np.array(l, "int")


# data1.3
def lines_cross_points(lines):
    k = 0
    point = [[0, 0], [0, 0], [0, 0], [0, 0]]
    point[0] = line2cross(lines[0], lines[1])
    point[1] = line2cross(lines[1], lines[2])
    point[2] = line2cross(lines[2], lines[3])
    point[3] = line2cross(lines[3], lines[0])
    return np.array(point)


# data1.2/1
# def lines_cross_points(lines):
#     k = 0
#     point = [[0, 0], [0, 0], [0, 0], [0, 0]]
#     for i in range(3):
#         for j in range(3 - i):
#             s = j + i + 1
#             line1 = lines[i]
#             line2 = lines[s]
#             [x0, y0] = line2cross(line1, line2)
#             if x0 != 1000:
#                 point[k] = [x0, y0]
#                 k = k + 1
#     return np.array(point)
def line2cross(line1, line2):
    [x1, y1, x2, y2] = line1
    [x3, y3, x4, y4] = line2
    b1 = (y2 - y1) * x1 + (x1 - x2) * y1
    b2 = (y4 - y3) * x3 + (x3 - x4) * y3
    D = (x2 - x1) * (y4 - y3) - (x4 - x3) * (y2 - y1)
    D1 = b2 * (x2 - x1) - b1 * (x4 - x3)
    D2 = b2 * (y2 - y1) - b1 * (y4 - y3)
    x0 = int(D1 / D)
    y0 = int(D2 / D)
    return [x0, y0]


# copy from py winform
def order_points(pts):
    # initialzie a list of coordinates that will be ordered
    # such that the first entry in the list is the top-left,
    # the second entry is the top-right, the third is the
    # bottom-right, and the fourth is the bottom-left
    rect = np.zeros((4, 2), dtype="float32")

    # the top-left point will have the smallest sum, whereas
    # the bottom-right point will have the largest sum
    s = pts.sum(axis=1)
    rect[0] = pts[np.argmin(s)]
    rect[2] = pts[np.argmax(s)]

    # now, compute the difference between the points, the
    # top-right point will have the smallest difference,
    # whereas the bottom-left will have the largest difference
    diff = np.diff(pts, axis=1)
    rect[1] = pts[np.argmin(diff)]
    rect[3] = pts[np.argmax(diff)]

    # return the ordered coordinates
    return rect


# action code
merge = 1
joint = 2
nope = 0


def find_similar_ls(lines):
    '''
    find indexes of 2 pallared ls
    :param lines:
    :return: [i,j] or [-1,-1]
    '''
    n = len(lines)
    for i in range(n):
        if lines[i].all() == 0: continue
        for j in range(i + 1, n):
            if lines[j].all() == 0: continue
            d = distance_ls_ls(lines[i], lines[j])
            t = delta_theta_ls(lines[i], lines[j])

            if t < max_angle_for_paralleled:
                if d < min_width_of_ls_to_merge:
                    return [merge, i, j]
                elif d < max_distance_of_ls_to_joint and basi_ls_in_same_line(lines[i],
                                                                              lines[j]) < max_basis_in_same_line:
                    return [joint, i, j]
    return [nope, -1, -1]


def remove_single_ls(lines):
    """
    remove single lines in the img, which are expected to exist in double, and are paralleled
    :param lines:
    :return: in-place action, [0000] will be assign to a line if it's abnormal
    """
    n = len(lines)
    is_doubling = False
    for i in range(n):
        if lines[i].all() == 0: continue
        for j in range(i + 1, n):
            if lines[j].all() == 0: continue
            d = distance_ls_ls(lines[i], lines[j])
            t = delta_theta_ls(lines[i], lines[j])
            if d > min_gap_for_doubling_lines and t < max_angle_for_paralleled:
                is_doubling = True
                break
        if not is_doubling:
            lines[i] = 0


def remove_slope(ls):
    """
    remove lines, which angle is greater than <max_angle_to_remove>
    :param ls:
    :return: remove count
    """
    c = 0
    for i in range(len(ls)):
        t = abs(theta_ls(ls[i]))
        tc = np.pi/2 - t
        t = min([t,tc])
        if t > max_angle_to_remove:
            ls[i] = 0
            c += 1
    return c


def merge_similar_ls(lines):
    '''
    merge similar lines seg, do fillit short lines before
    :param lines:
    :return:
    '''
    lines = np.array(lines).reshape(-1, 4)
    remove_short_ls(lines)
    remove_slope(lines)
    # start iteration
    [action, i, j] = find_similar_ls(lines)
    while action != nope:
        if action == merge:
            # lines[i] = (lines[i] + lines[j]) / 2  # merge and delete
            lines[i] = merge_ls(lines[i], lines[j])
            # print('merge')
        elif action == joint:
            lines[i] = joint_ls(lines[i], lines[j])
            # print('joint')

        lines[j] = 0
        [action, i, j] = find_similar_ls(lines)
    remove_single_ls(lines)
    lss = []
    for l in lines:
        if l.any() != 0:
            lss.append(l)
    return np.array(lss, 'int')
